
#include "cmd_func.h"
#include "stdlib.h"
#include "../uart/oslib_uart.h"
#include "FreeRTOS.h"
#include "task.h"

#include "../UserFunc/dynamixel2.h"
#include "../UserFunc/servo.h"
#include "../UserFunc/Dog.h"
#include "../UserFunc/utils.h"
#include "../UserFunc/state.h" // robot dog state_machine debug
#include "tim.h"
#include "../UserFunc/HX711.h"
#include "../UserFunc/flash.h"

#ifdef OSLIB_CAN_MODULE_ENABLED
#include "../can/oslib_can.h"
#endif

#if defined(OSLIB_UART_MODULE_ENABLED) && USE_OSLIB_UART_CLI
/* 在这里添加命令回调函数的定义或外部声明 */
static void Command_Hello(int argc, char *argv[])
{
    uprintf("Hello OSLib!\r\n");
}

static void Command_Echo(int argc, char *argv[])
{
    for (int i = 1; i < argc; i++)
        uprintf("Echo: %s\r\n", argv[i]);
}

#ifdef OSLIB_CAN_MODULE_ENABLED
static void Command_CanSend(int argc, char *argv[])
{
    if (argc >= 3)
    {
        can_msg msg;
        msg.in[0] = 0x40005850;
        msg.in[1] = 0x00000000;
        uint32_t id = strtol(argv[2], NULL, 16);
        if (argv[1][0] == 's')
        {
            can_send_msg(id, &msg);
            uprintf("CanSend:std[%x]\r\n", id);
        }
        else if (argv[1][0] == 'e')
        {
            can_ext_send_msg(id, &msg);
            uprintf("CanSend:ext[%x]\r\n", id);
        }
        else
            uprintf("Param Error!\r\n");
    }
    else
    {
        uprintf("Param Error!\r\n");
    }
}
#endif

static void Command_Task(int argc, char *argv[])
{
    uprintf("Free Heap: %dB\r\n", xPortGetFreeHeapSize());
#if ((configUSE_TRACE_FACILITY == 1) && (configUSE_STATS_FORMATTING_FUNCTIONS > 0) && (configSUPPORT_DYNAMIC_ALLOCATION == 1))
    static char tasklist_buffer[256];
    uprintf("Name          \tState\tPrio\tStack\tRank\r\n");
    vTaskList(tasklist_buffer);
    uprintf(tasklist_buffer);
#endif
    // uprintf("Name          \tCount\tUtilization\r\n");
    // vTaskGetRunTimeStats(tasklist_buffer);
    // uprintf(tasklist_buffer);
}

static void Command_ShowBroadcast(int argc, char *argv[])
{
    // show_broadcast();
    uprintf("OpenMV angle %d\n\r", ANGLE);
}

static void Command_Half(int argc, char *argv[])
{
    if (strcmp(argv[1], "led") == 0)
    {
        Servo_Led(1, 1);
    }
    else if (strcmp(argv[1], "leg_reset") == 0)
    {
        uint8_t id = (uint8_t)(atoi(argv[2]));
        Servo_Sync_ResetLeg(id);
    }
    else if (strcmp(argv[1], "tor") == 0)
    {
        int id = (atoi(argv[2]));
        bool TrqState = (bool)(atoi(argv[3]));
        if (id == -1)
        {
            Servo_Sync_Torque(TrqState);
        }
        else
        {
            Servo_Torque((uint8_t)id, TrqState);
            uprintf("id %d tor %d\n\r", id, TrqState);
        }
    }
    else if (strcmp(argv[1], "pos") == 0)
    {
        uint8_t id = (uint8_t)(atoi(argv[2]));
        uint32_t pos = (uint32_t)(atoi(argv[3]));
        Servo_Goal_Position(id, pos);
        uprintf("id %d pos %d\n\r", id, pos);
    }

    else if (strcmp(argv[1], "sync_pos") == 0)
    {
        float ang[12] = {0};
        ang[0] = atof(argv[2]);
        ang[1] = atof(argv[3]);
        ang[2] = atof(argv[4]);
        ang[3] = atof(argv[5]);
        ang[4] = atof(argv[6]);
        ang[5] = atof(argv[7]);
        ang[6] = atof(argv[8]);
        ang[7] = atof(argv[9]);
        ang[8] = atof(argv[10]);
        ang[9] = atof(argv[11]);
        ang[10] = atof(argv[12]);
        ang[11] = atof(argv[13]);
        uint32_t pos[12] = {2048};
        pos[0] = (uint32_t)(-11.36 * ang[0] + 2048);
        pos[1] = (uint32_t)(-11.38 * ang[1] + 2048);
        pos[2] = (uint32_t)(-11.38 * ang[2] + 2048);
        pos[3] = (uint32_t)(-11.36 * ang[3] + 2048);
        pos[4] = (uint32_t)(11.38 * ang[4] + 2048);
        pos[5] = (uint32_t)(11.38 * ang[5] + 2048);
        pos[6] = (uint32_t)(11.36 * ang[6] + 2048);
        pos[7] = (uint32_t)(11.38 * ang[7] + 2048);
        pos[8] = (uint32_t)(11.38 * ang[8] + 2048);
        pos[9] = (uint32_t)(11.36 * ang[9] + 2048);
        pos[10] = (uint32_t)(-11.38 * ang[10] + 2048);
        pos[11] = (uint32_t)(-11.38 * ang[11] + 2048);
        Servo_Sync_Goal_Position(pos);
        uprintf("sync_pos\n\r");
    }

    else if (strcmp(argv[1], "leg_pos") == 0)
    {
        uint8_t id = (uint8_t)(atoi(argv[2]));
        float end_x = atof(argv[3]);
        float end_y = atof(argv[4]);
        float end_z = atof(argv[5]);
        // uprintf("%f %f %f \n\r", atof(argv[3]), atof(argv[4]), atof(argv[5]));
        Dog.legs[id].foot_target_x = end_x;
        Dog.legs[id].foot_target_y = end_y;
        Dog.legs[id].foot_target_z = end_z;
        Dog.legs[id].configuration = atoi(argv[6]);
        uprintf("foot:(%f %f %f)\n\r", Dog.legs[id].foot_target_x, Dog.legs[id].foot_target_y, Dog.legs[id].foot_target_z);
        Dog_Inverse_kinematics(&Dog.legs[id]);
        Dog_servo_angle_pos();
        Dog.ctrl_mode = DOG_MODE_DEBUG;
        uprintf("inverse reslt %f %f %f\n\r", Dog.legs[id].servo[0].goal_angle, Dog.legs[id].servo[1].goal_angle, Dog.legs[id].servo[2].goal_angle);
    }
}
static void Command_Flash(int argc, char *argv[])
{
    if (strcmp(argv[1], "read") == 0)
    {
        uprintf("read flash...\n\r");
        STMFLASH_Read(FLASH_SAVE_ADDR, (uint32_t *)dog_flash_data, DOG_FLASH_DATA_NUM);
        uprintf("flash_data[0] (GAIT_S)       %.3f\n\r", dog_flash_data[0]);
        uprintf("flash_data[1] (GAIT_H)       %.3f\n\r", dog_flash_data[1]);
        uprintf("flash_data[2] (GAIT_T)       %.3f\n\r", dog_flash_data[2]);
        uprintf("flash_data[3] (gait_delta_x) %.3f\n\r", dog_flash_data[3]);
        uprintf("flash_data[4] (gait_delta_z) %.3f\n\r", dog_flash_data[4]);
        uprintf("flash_data[5] (delta_h)      %.3f\n\r", dog_flash_data[5]);
        uprintf("flash_data[6] (delta_z)      %.3f\n\r", dog_flash_data[6]);
        uprintf("flash_data[7] (delta_x)      %.3f\n\r", dog_flash_data[7]);
        uprintf("flash_data[8] (delta_t)      %.3f\n\r", dog_flash_data[8]);
        uprintf("flash_data[9] (delta_dh[0])  %.3f\n\r", dog_flash_data[9]);
        uprintf("flash_data[10](delta_dh[1])  %.3f\n\r", dog_flash_data[10]);
        uprintf("flash_data[11](delta_dh[2])  %.3f\n\r", dog_flash_data[11]);
        uprintf("flash_data[12](delta_dh[3])  %.3f\n\r", dog_flash_data[12]);
        uprintf("flash_data[13] (delta_z_front)      %.3f\n\r", dog_flash_data[13]);
        uprintf("flash_data[14] (delta_z_back)      %.3f\n\r", dog_flash_data[14]);
        uprintf("Done!\n\r");
        uprintf("If you want to use these param,enter 'flash load'\n\r");
    }
    else if (strcmp(argv[1], "load") == 0)
    {
        uprintf("load param from flash...\n\r");
        STMFLASH_Read(FLASH_SAVE_ADDR, (uint32_t *)dog_flash_data, DOG_FLASH_DATA_NUM);
        GAIT_S = dog_flash_data[0];
        GAIT_H = dog_flash_data[1];
        GAIT_T = dog_flash_data[2];
        gait_delta_x = dog_flash_data[3];
        gait_delta_z = dog_flash_data[4];
        delta_h = dog_flash_data[5];
        delta_z = dog_flash_data[6];
        delta_x = dog_flash_data[7];
        delta_t = dog_flash_data[8];
        delta_dh[0] = dog_flash_data[9];
        delta_dh[1] = dog_flash_data[10];
        delta_dh[2] = dog_flash_data[11];
        delta_dh[3] = dog_flash_data[12];
        delta_z_front = dog_flash_data[13];
        delta_z_back = dog_flash_data[14];
        uprintf("Done!\n\r");
    }
    else if (strcmp(argv[1], "show") == 0)
    {
        uprintf("Current dog parameters\n\r");
        uprintf("GAIT_S       %.3f\n\r", GAIT_S);
        uprintf("GAIT_H       %.3f\n\r", GAIT_H);
        uprintf("GAIT_T       %.3f\n\r", GAIT_T);
        uprintf("gait_delta_x %.3f\n\r", gait_delta_x);
        uprintf("gait_delta_z %.3f\n\r", gait_delta_z);
        uprintf("delta_h      %.3f\n\r", delta_h);
        uprintf("delta_z      %.3f\n\r", delta_z);
        uprintf("delta_x      %.3f\n\r", delta_x);
        uprintf("delta_t      %.3f\n\r", delta_t);
        uprintf("delta_dh[0]  %.3f\n\r", delta_dh[0]);
        uprintf("delta_dh[1]  %.3f\n\r", delta_dh[1]);
        uprintf("delta_dh[2]  %.3f\n\r", delta_dh[2]);
        uprintf("delta_dh[3]  %.3f\n\r", delta_dh[3]);
        uprintf("delta_z_front      %.3f\n\r", delta_z_front);
        uprintf("delta_z_back      %.3f\n\r", delta_z_back);
        uprintf("If you want to save these param,enter 'flash save'\n\r");
    }
    else if (strcmp(argv[1], "save") == 0)
    {
        uprintf("save param to flash...\n\r");
        dog_flash_data[0] = GAIT_S;
        dog_flash_data[1] = GAIT_H;
        dog_flash_data[2] = GAIT_T;
        dog_flash_data[3] = gait_delta_x;
        dog_flash_data[4] = gait_delta_z;
        dog_flash_data[5] = delta_h;
        dog_flash_data[6] = delta_z;
        dog_flash_data[7] = delta_x;
        dog_flash_data[8] = delta_t;
        dog_flash_data[9] = delta_dh[0];
        dog_flash_data[10] = delta_dh[1];
        dog_flash_data[11] = delta_dh[2];
        dog_flash_data[12] = delta_dh[3];
        dog_flash_data[13] = delta_z_front;
        dog_flash_data[14] = delta_z_back;
        STMFLASH_Write(FLASH_SAVE_ADDR, (uint32_t *)dog_flash_data, DOG_FLASH_DATA_NUM);
        uprintf("Done!\n\r");
    }
}
static void Command_climb(int argc, char *argv[])
{
    climb_first_flag = atoi(argv[1]);
    uprintf("climb_first_flag = %d\n\r", climb_first_flag);
}
static void Command_PID(int argc, char *argv[])
{
    if (strcmp(argv[1], "roll") == 0)
    {
        RollPID.KP = atof(argv[2]);
        RollPID.KI = atof(argv[3]);
        RollPID.KD = atof(argv[4]);
        print_PID(&RollPID);
    }
    else if (strcmp(argv[1], "pitch") == 0)
    {
        Pitch_PID.KP = atof(argv[2]);
        Pitch_PID.KI = atof(argv[3]);
        Pitch_PID.KD = atof(argv[4]);
        print_PID(&Pitch_PID);
    }
    else if (strcmp(argv[1], "turn") == 0)
    {
        TurnPID.KP = atof(argv[2]);
        TurnPID.KI = atof(argv[3]);
        TurnPID.KD = atof(argv[4]);
        print_PID(&TurnPID);
    }
}
static void Command_Dog(int argc, char *argv[])
{
    if (strcmp(argv[1], "help") == 0)
    {
        uprintf("dog\n\r");
        uprintf("  |--help:     show this page\n\r");
        uprintf("  |--ginit:     Dog gait init\n\r");
        uprintf("  |--mode:     chage run mode\n\r");
        uprintf("  |--state:    show dog state\n\r");
        uprintf("  |--reinit:    foot_target -> foot_init\n\r");
        uprintf("  |--adjust:   for 'Dog_Pose_Adjust()'\n\r");
        uprintf("  |   |--x\n\r");
        uprintf("  |   |--y\n\r");
        uprintf("  |   |--z\n\r");
        uprintf("  |   |--Y\n\r");
        uprintf("  |   |--P\n\r");
        uprintf("  |   |--R\n\r");
        uprintf("  |   |--adjust\n\r");
        uprintf("  |--init:     Fine tune the initial foot position\n\r");
        uprintf("  |   |--id\n\r");
        uprintf("  |   |   |--x\n\r");
        uprintf("  |   |   |--y\n\r");
        uprintf("  |   |   |--z\n\r");
        uprintf("  |--weight:   force sensor\n\r");
        uprintf("  |   |--show\n\r");
        uprintf("  |   |--init\n\r");
        uprintf("  |--delta:   \n\r");
        uprintf("  |   |--show\n\r");
        uprintf("  |   |--h\n\r");
        uprintf("  |   |--z\n\r");
        uprintf("  |   |--zb\n\r");
        uprintf("  |   |--zf\n\r");
        uprintf("  |   |--x\n\r");
        uprintf("  |   |--t\n\r");
        uprintf("  |--param:  GAIT_xxx \n\r");
        uprintf("  |   |--show\n\r");
        uprintf("  |   |--s\n\r");
        uprintf("  |   |--h\n\r");
        uprintf("  |   |--t\n\r");
        uprintf("  |   |--gdx\n\r");
        uprintf("  |   |--gdz\n\r");
        uprintf("  |--bc:   barycenter\n\r");
        uprintf("  |   |--front\n\r");
        uprintf("  |   |--back\n\r");
        uprintf("  |   |--left\n\r");
        uprintf("  |   |--right\n\r");
        uprintf("  |   |--central\n\r");
        uprintf("  |   |--show\n\r");
        uprintf("  |--config:   leg config\n\r");
        uprintf("  |--phase:   leg phase\n\r");
        uprintf("  |--turn:   Note that it is incremental\n\r");
        uprintf("  |   |--dhr\n\r");
        uprintf("  |   |--dhl\n\r");
    }
    else if (strcmp(argv[1], "vx") == 0)
    {
        Dog.v_x = atof(argv[2]);
        uprintf("Dog vx %.3f\n\r", Dog.v_x);
    }
    else if (strcmp(argv[1], "vz") == 0)
    {
        Dog.v_z = atof(argv[2]);
        uprintf("Dog vz %.3f\n\r", Dog.v_z);
    }
    else if (strcmp(argv[1], "vyaw") == 0)
    {
        Dog.target_yaw = atof(argv[2]);
        uprintf("Dog vyaw %.3f\n\r", Dog.target_yaw);
    }
    else if (strcmp(argv[1], "ginit") == 0)
    {
        Dog_Gait_Init();
        uprintf("Dog gait init\n\r");
    }
    else if (strcmp(argv[1], "mode") == 0)
    {
        Dog.ctrl_mode = atoi(argv[2]);
        uprintf("switch dog mode to %d\n\r", Dog.ctrl_mode);
    }
    else if (strcmp(argv[1], "reinit") == 0)
    {
        Dog_Gait_Reinit();
        uprintf("Dog Gait Reinit\n\r");
    }
    else if (strcmp(argv[1], "state") == 0)
    {
        uprintf("p0(%f,%f,%f) p1(%f,%f,%f) p2(%f,%f,%f) p3(%f,%f,%f)\n\r", Dog.legs[0].foot_target_x, Dog.legs[0].foot_target_y, Dog.legs[0].foot_target_z, Dog.legs[1].foot_target_x, Dog.legs[1].foot_target_y, Dog.legs[1].foot_target_z, Dog.legs[2].foot_target_x, Dog.legs[2].foot_target_z, Dog.legs[2].foot_target_z, Dog.legs[3].foot_target_x, Dog.legs[3].foot_target_y, Dog.legs[3].foot_target_z);
        uprintf("now YPR:(%f,%f,%f)\n\r", Dog.now_yaw, Dog.now_pitch, Dog.now_roll);
        uprintf("target YPR:(%f,%f,%f)\n\r", Dog.target_yaw, Dog.target_pitch, Dog.target_roll);
        uprintf("target xyz:(%f,%f,%f)\n\r", Dog.target_x, Dog.target_y, Dog.target_z);
        uprintf("now acc:(%f,%f,%f)\n\r", Dog.now_acc_x, Dog.now_acc_y, Dog.now_acc_z);
        uprintf("\n\r");
    }
    else if (strcmp(argv[1], "adjust") == 0)
    {
        if (strcmp(argv[2], "x") == 0)
        {
            Dog.target_x = atof(argv[3]);
        }
        else if (strcmp(argv[2], "y") == 0)
        {
            Dog.target_y = atof(argv[3]);
        }
        else if (strcmp(argv[2], "z") == 0)
        {
            Dog.target_z = atof(argv[3]);
        }
        else if (strcmp(argv[2], "Y") == 0)
        {
            Dog.target_yaw = atof(argv[3]);
        }
        else if (strcmp(argv[2], "P") == 0)
        {
            Dog.target_pitch = atof(argv[3]);
        }
        else if (strcmp(argv[2], "R") == 0)
        {
            Dog.target_roll = atof(argv[3]);
        }
        // Dog.pose_mode = atof(argv[2]);
        // Dog.target_x = atof(argv[3]);
        // Dog.target_y = atof(argv[4]);
        // Dog.target_z = atof(argv[5]);
        // Dog.target_roll = atof(argv[6]);S
        // Dog.target_pitch = atof(argv[7]);
        // Dog.target_yaw = atof(argv[8]);
        else if (strcmp(argv[2], "adjust") == 0)
        {
            Dog.ctrl_mode = DOG_MODE_ADJUST;
            Dog_Pose_Adjust();
        }
        uprintf("Dog pose[%d] adjust x:%f y:%f z:%f R:%f P:%f Y:%f\n\r", Dog.pose_mode, Dog.target_x, Dog.target_y, Dog.target_z, Dog.target_roll, Dog.target_pitch, Dog.target_yaw);
    }
    else if (strcmp(argv[1], "init") == 0)
    {
        int id = atoi(argv[2]);
        if (strcmp(argv[3], "x") == 0)
        {
            Dog.legs[id].foot_init_x += atof(argv[4]);
        }
        else if (strcmp(argv[3], "y") == 0)
        {
            Dog.legs[id].foot_init_y += atof(argv[4]);
        }
        else if (strcmp(argv[3], "z") == 0)
        {
            Dog.legs[id].foot_init_z += atof(argv[4]);
        }
        Dog.ctrl_mode = 2;
        uprintf("leg[%d] init x:%.4f y:%.4f z:%.4f\n\r", id, Dog.legs[id].foot_init_x, Dog.legs[id].foot_init_y, Dog.legs[id].foot_init_z);
    }
    else if (strcmp(argv[1], "weight") == 0)
    {
        if (strcmp(argv[2], "show") == 0)
        {
            uprintf("F0 %d | F1 %d\n\rF3 %d | F2 %d\n\r", Weight_Shiwu[0], Weight_Shiwu[1], Weight_Shiwu[3], Weight_Shiwu[2]);
        }
        else if (strcmp(argv[2], "init") == 0)
        {
            Weight_init();
            uprintf("F0 %d | F1 %d\n\rF3 %d | F2 %d\n\r", Weight_Shiwu[0], Weight_Shiwu[1], Weight_Shiwu[3], Weight_Shiwu[2]);
        }
    }
    else if (strcmp(argv[1], "delta") == 0)
    {
        if (strcmp(argv[2], "show") == 0)
        {
            uprintf("delta_z\t=    %f\n", delta_z);
            uprintf("delta_z_front\t=    %f\n", delta_z_front);
            uprintf("delta_z_back\t=    %f\n", delta_z_back);
            uprintf("delta_h\t=    %f\n", delta_h);
            uprintf("delta_t\t=    %f\n", delta_t);
            uprintf("delta_x\t=    %f\n", delta_x);
            uprintf("delta_dh[0]\t=    %f\n", delta_dh[0]);
            uprintf("delta_dh[1]\t=    %f\n", delta_dh[1]);
            uprintf("delta_dh[3]\t=    %f\n", delta_dh[3]);
            uprintf("delta_dh[2]\t=    %f\n", delta_dh[2]);
        }
        else if (strcmp(argv[2], "z") == 0)
        {
            delta_z = atof(argv[3]);
            uprintf("delta_z\t=    %f\n", delta_z);
        }
        else if (strcmp(argv[2], "zf") == 0)
        {
            delta_z_front = atof(argv[3]);
            uprintf("delta_z_front\t=    %f\n", delta_z_front);
        }
        else if (strcmp(argv[2], "zb") == 0)
        {
            delta_z_back = atof(argv[3]);
            uprintf("delta_z_back\t=    %f\n", delta_z_back);
        }
        else if (strcmp(argv[2], "x") == 0)
        {
            delta_x = atof(argv[3]);
            uprintf("delta_x\t=    %f\n", delta_x);
        }
        else if (strcmp(argv[2], "h") == 0)
        {
            delta_h = atof(argv[3]);
            uprintf("delta_h\t=    %f\n", delta_h);
        }
        else if (strcmp(argv[2], "t") == 0)
        {
            delta_t = atof(argv[3]);
            uprintf("delta_t\t=    %f\n", delta_t);
        }
        else if (strcmp(argv[2], "dhlf") == 0)
        {
            delta_dh[0] = atof(argv[3]);
            uprintf("delta_dh[0]\t=    %f\n", delta_dh[0]);
        }
        else if (strcmp(argv[2], "dhrf") == 0)
        {
            delta_dh[1] = atof(argv[3]);
            uprintf("delta_dh[1]\t=    %f\n", delta_dh[1]);
        }
        else if (strcmp(argv[2], "dhlb") == 0)
        {
            delta_dh[3] = atof(argv[3]);
            uprintf("delta_dh[3]\t=    %f\n", delta_dh[3]);
        }
        else if (strcmp(argv[2], "dhrb") == 0)
        {
            delta_dh[2] = atof(argv[3]);
            uprintf("delta_dh[2]\t=    %f\n", delta_dh[2]);
        }
    }
    else if (strcmp(argv[1], "param") == 0)
    {
        if (strcmp(argv[2], "show") == 0)
        {
            uprintf("GAIT_S\t=    %f\n", GAIT_S);
            uprintf("GAIT_H\t=    %f\n", GAIT_H);
            uprintf("GAIT_T\t=    %f\n", GAIT_T);
            uprintf("gait_delta_x\t=    %f\n", gait_delta_x);
            uprintf("gait_delta_y\t=    %f\n", gait_delta_y);
            uprintf("gait_delta_z\t=    %f\n", gait_delta_z);
        }
        else if (strcmp(argv[2], "s") == 0)
        {
            GAIT_S = atof(argv[3]);
            uprintf("GAIT_S\t=    %f\n", GAIT_S);
        }
        else if (strcmp(argv[2], "h") == 0)
        {
            GAIT_H = atof(argv[3]);
            uprintf("GAIT_H\t=    %f\n", GAIT_H);
        }
        else if (strcmp(argv[2], "t") == 0)
        {
            GAIT_T = atof(argv[3]);
            uprintf("GAIT_T\t=    %f\n", GAIT_T);
        }
        else if (strcmp(argv[2], "gdx") == 0)
        {
            gait_delta_x = atof(argv[3]);
            uprintf("gait_delta_x\t=    %f\n", gait_delta_x);
        }
        else if (strcmp(argv[2], "gdy") == 0)
        {
            gait_delta_y = atof(argv[3]);
            uprintf("gait_delta_y\t=    %f\n", gait_delta_y);
        }
        else if (strcmp(argv[2], "gdz") == 0)
        {
            gait_delta_z = atof(argv[3]);
            uprintf("gait_delta_z\t=    %f\n", gait_delta_z);
        }
    }

    else if (strcmp(argv[1], "bc") == 0)
    {
        if (strcmp(argv[2], "f") == 0)
        {
            barycenter_front = atof(argv[3]);
            uprintf("barycenter_front = %f\n\r", barycenter_front);
        }
        else if (strcmp(argv[2], "b") == 0)
        {
            barycenter_back = atof(argv[3]);
            uprintf("barycenter_back = %f\n\r", barycenter_back);
        }
        else if (strcmp(argv[2], "l") == 0)
        {
            barycenter_left = atof(argv[3]);
            uprintf("barycenter_left = %f\n\r", barycenter_left);
        }
        else if (strcmp(argv[2], "r") == 0)
        {
            barycenter_right = atof(argv[3]);
            uprintf("barycenter_right = %f\n\r", barycenter_right);
        }
        else if (strcmp(argv[2], "c") == 0)
        {
            barycenter_central = atof(argv[3]);
            uprintf("barycenter_central = %f\n\r", barycenter_central);
        }
        else if (strcmp(argv[2], "show") == 0)
        {
            uprintf("barycenter_front = %f\n\r", barycenter_front);
            uprintf("barycenter_back = %f\n\r", barycenter_back);
            uprintf("barycenter_left = %f\n\r", barycenter_left);
            uprintf("barycenter_right = %f\n\r", barycenter_right);
            uprintf("barycenter_central = %f\n\r", barycenter_central);
        }
    }
    else if (strcmp(argv[1], "config") == 0)
    {
        for (int i = 0; i < 4; i++)
        {
            Dog.legs[i].configuration = atoi(argv[i + 2]);
        }
        uprintf("leg[0] %d | leg[1] %d\n\rleg[3] %d | leg[2] %d\n\r", Dog.legs[0].configuration, Dog.legs[1].configuration, Dog.legs[3].configuration, Dog.legs[2].configuration);
    }
    else if (strcmp(argv[1], "phase") == 0)
    {
        for (int i = 0; i < 4; i++)
        {
            Dog.phase[i] = atoi(argv[i + 2]);
        }
        float phase_graph_tmp[4][2] = {{1, 1}, {-1, 1}, {-1, -1}, {1, -1}};
        for (int i = 0; i < 4; i++)
        {
            phase_graph[i][0] = phase_graph_tmp[Dog.phase[i]][0];
            phase_graph[i][1] = phase_graph_tmp[Dog.phase[i]][1];
        }
        uprintf("Dog phase: %d %d %d %d\n\r", Dog.phase[0], Dog.phase[1], Dog.phase[2], Dog.phase[3]);
        uprintf("phase_graph {%.1f,%.1f} {%.1f,%.1f} {%.1f,%.1f} {%.1f,%.1f}\n\r", phase_graph[0][0], phase_graph[0][1], phase_graph[1][0], phase_graph[1][1], phase_graph[2][0], phase_graph[2][1], phase_graph[3][0], phase_graph[3][1]);
    }
    else if (strcmp(argv[1], "turn") == 0)
    {
        if (strcmp(argv[2], "show") == 0)
        {
            uprintf("Turn \n\r");
            uprintf("delta_dh[0]\t=    %f\n", delta_dh[0]);
            uprintf("delta_dh[1]\t=    %f\n", delta_dh[1]);
            uprintf("delta_dh[2]\t=    %f\n", delta_dh[2]);
            uprintf("delta_dh[3]\t=    %f\n", delta_dh[3]);
            Dog_Gait_Turn();
        }
        else
        {
            delta_dh[0] = atof(argv[2]);
            delta_dh[1] = atof(argv[3]);
            delta_dh[2] = atof(argv[4]);
            delta_dh[3] = atof(argv[5]);
            Dog_Gait_Turn();
            uprintf("Turn \n\r");
            uprintf("delta_dh[0]\t=    %f\n", delta_dh[0]);
            uprintf("delta_dh[1]\t=    %f\n", delta_dh[1]);
            uprintf("delta_dh[2]\t=    %f\n", delta_dh[2]);
            uprintf("delta_dh[3]\t=    %f\n", delta_dh[3]);
        }
    }
    else if (strcmp(argv[1], "debug") == 0)
    {
        Dog.ctrl_mode = DOG_MODE_DEBUG;
        debug_leg = atoi(argv[2]);
        debug_target_x = atof(argv[3]);
        debug_target_y = atof(argv[4]);
        debug_target_z = atof(argv[5]);
        uprintf("debug leg[%d] (%.3f,%.3f,%.3f)", debug_leg, debug_target_x, debug_target_y, debug_target_z);
    }
    else if (strcmp(argv[1], "omni") == 0)
    {
        Dog.v_x = atof(argv[2]);
        Dog.v_z = atof(argv[3]);
        Dog_Gait_Walk_Omni();
        uprintf("omni \n\r");
        uprintf("Dog.v_x\t=    %f\n", Dog.v_x);
        uprintf("Dog.v_z\t=    %f\n", Dog.v_z);
    }
}

static void Command_Dog_state(int argc, char *argv[])
// robot dog state_machine debug
{
    // 开始执行 状态机
    if (strcmp(argv[1], "mode") == 0)
    {
        state = atoi(argv[2]);
        uprintf("switch state mode to %d\n\r", state);
    }
    else if (strcmp(argv[1], "1") == 0)
    {
        Dog.ctrl_mode = DOG_MODE_RESET;
        osDelay(187);
        Dog.ctrl_mode = DOG_MODE_WALK;
        state = state_1_begin;
        uprintf("set state 1 success\n\r");
    }
    else if (strcmp(argv[1], "climb") == 0)
    {
        Dog.ctrl_mode = DOG_MODE_RESET;
        osDelay(187);
        Dog.legs[0].configuration = 1;
        Dog.legs[1].configuration = 1;
        Dog.legs[2].configuration = 1;
        Dog.legs[3].configuration = 1;
    }
    // debug 模式
    else if (strcmp(argv[1], "0") == 0)
    {
        // Dog_Gait_Reinit();
        Dog.ctrl_mode = DOG_MODE_RESET;
        state = state_debug;
        uprintf("reset !!\n\r");
    }
    else if (strcmp(argv[1], "print") == 0) // 打印openmv信息
    {
        for (int i = 0; i < 8; i++)
        {
            uprintf("%d ", UART3_TaskBuffer[i]);
        }
        uprintf("\r\n");
    }
    else if (strcmp(argv[1], "open") == 0) // 测试舵机
    {
        uprintf("open!!\r\n");
        action_open();
        // int turn_time = 0;
        // turn_time = atoi(argv[2]);
        // __HAL_TIM_SET_COMPARE(&htim4, TIM_CHANNEL_1, turn_time); // 关门
    }
    else if (strcmp(argv[1], "adjust") == 0) // 直线调整
    {
        DIRECTION = atoi(argv[2]);
        // adjust_direction();
        uprintf("adjust direction \n\r");
        uprintf("Dog direction\t=    %d\n", DIRECTION);
    }
    else if (strcmp(argv[1], "turn") == 0) // 转弯调整
    {
        DIRECTION = atoi(argv[2]);
        adjust_turn();
        uprintf("adjust turn \n\r");
        uprintf("Dog direction\t=    %d\n", DIRECTION);
    }
}

/* 在这里完成命令到回调函数的映射 */
void Command_Init()
{
    OSLIB_UartCmd_AddCommand("hello", "\"Hello OSLib!\"", Command_Hello); /* 输出 "Hello SimpleLib!" */ // 测试串口输出和命令
    OSLIB_UartCmd_AddCommand("echo", "Echo message", Command_Echo); /* 回显参数的内容 */                // 测试命令参数解析
#ifdef OSLIB_CAN_MODULE_ENABLED
    OSLIB_UartCmd_AddCommand("cansend", "CAN send message", Command_CanSend); /* 发送CAN报文, 内容为两个整数 */ // 测试CAN发送
#endif
    OSLIB_UartCmd_AddCommand("tasks", "Show task list", Command_Task);
    OSLIB_UartCmd_AddCommand("half", "", Command_Half);
    OSLIB_UartCmd_AddCommand("show", "", Command_ShowBroadcast);
    OSLIB_UartCmd_AddCommand("state", "", Command_Dog_state);
    OSLIB_UartCmd_AddCommand("dog", "", Command_Dog);
    OSLIB_UartCmd_AddCommand("flash", "", Command_Flash);
    OSLIB_UartCmd_AddCommand("pid", "", Command_PID);
    OSLIB_UartCmd_AddCommand("climb", "", Command_climb);
}

#endif
